/*
 * PR.c
 *
 *  Created on: Jun 10, 2024
 *      Author: Liangmiyi
 */

#include <MY_INC/PHY/PHY_PR.h>

/*==================================普通PR算法===================================*/

/**
 * @brief 普通PR算法初始化函数
 * @param PR PR结构体指针
 * @param Kp Kp 参数，对应速度增益，速度控制速度环
 * @param Kr Kr 参数，对应静态增益，速度控制电流环
 * @param w0 w0 参数，频率
 * @param ts ts 执行周期，单位秒
 * @param wc wc 参数
 */
void PR_Calculate_Init(PR_Typedef *PR, float Kp, float Kr, float w0, float wc, float ts)
{
    PR->Kp = Kp;
    PR->Kr = Kr;
    PR->wc = wc;
    PR->w0 = w0;
    PR->ts = ts;

    PR->temp1 = 2.0f * wc * Kr;
    PR->temp2 = 2.0f * wc;
    PR->w0_2_ts = w0 * w0 * ts;
}

/**
 * @brief 普通PR算法计算函数
 * @param PR PR结构体指针
 * @param PR_Target 目标电压
 * @param PR_Actual 当前电压
 * @return PR算法输出值
 */
float PR_Calculate(PR_Typedef *PR, float PR_Target, float PR_Actual)
{
    /*===================== PR 计算 =====================*/
    // 计算误差
    PR->error = PR_Target - PR_Actual;
    // 计算前馈积分器输入
    PR->input_of_forward_integrator = (PR->temp1 * PR->error) - PR->output_of_feedback;
    // 计算前馈积分器输出
    PR->output_of_forward_integrator += PR->ts * PR->input_of_forward_integrator;
    // 计算后馈积分器输出
    PR->output_of_backward_integrator += PR->output_of_forward_integrator * PR->w0_2_ts;
    // 计算反馈输出
    PR->output_of_feedback = PR->output_of_backward_integrator + PR->temp2 * PR->output_of_forward_integrator;
    // 计算最终输出
    PR->Output = (PR->output_of_forward_integrator + PR->Kp * PR->error);
    return PR->Output;
}

/**
 * @brief 谐波抑制PR算法计算函数
 * @param PR PR结构体指针
 * @param PR_Target 目标电压
 * @param PR_Actual 当前电压
 * @return PR算法输出值
 */
float PR_Harmonic_Calculate(PR_Typedef *PR[], float PR_Target, float PR_Actual)
{
    /*===================== 单次 PR 计算 =====================*/
    float Out = 0.0f;
    // 计算误差
    float error = PR_Target - PR_Actual;
    for (uint8_t i = 0; i < Harmonic_Order; i++)
    {
        // 计算前馈积分器输入
        PR[i]->input_of_forward_integrator = (PR[i]->temp1 * error) - PR[i]->output_of_feedback;
        // 计算前馈积分器输出
        PR[i]->output_of_forward_integrator += PR[i]->ts * PR[i]->input_of_forward_integrator;
        // 计算后馈积分器输出
        PR[i]->output_of_backward_integrator += PR[i]->output_of_forward_integrator * PR[i]->w0_2_ts;
        // 计算反馈输出
        PR[i]->output_of_feedback =
            PR[i]->output_of_backward_integrator + PR[i]->temp2 * PR[i]->output_of_forward_integrator;
        // 计算最终输出
        PR[i]->Output = (PR[i]->output_of_forward_integrator + PR[i]->Kp * error);
        Out += PR[i]->Output;
    }
    return Out;
}
float PMR_Calculate(PR_Typedef *PR[], float PR_Target, float PR_Actual)
{
    /*===================== 单次 PR 计算 =====================*/
    float Out = 0.0f;
    // 计算误差
    float error = PR_Target - PR_Actual;
    float Kp_Tmep = error * PR[0]->Kp;
    error = 2.0f * error * PR[0]->wc;
    for (uint8_t i = 0; i < Harmonic_Order; i++)
    {
        // 计算前馈积分器输入
        PR[i]->input_of_forward_integrator = (PR[i]->temp1 * error) - PR[i]->output_of_feedback;
        // 计算前馈积分器输出
        PR[i]->output_of_forward_integrator += PR[i]->ts * PR[i]->input_of_forward_integrator;
        // 计算后馈积分器输出
        PR[i]->output_of_backward_integrator += PR[i]->output_of_forward_integrator * PR[i]->w0_2_ts;
        // 计算反馈输出
        PR[i]->output_of_feedback =
            PR[i]->output_of_backward_integrator + PR[i]->temp2 * PR[i]->output_of_forward_integrator;
        // 计算最终输出
        PR[i]->Output = (PR[i]->output_of_forward_integrator);
        Out += PR[i]->Output;
    }
    return Out + Kp_Tmep;
}
/*==================================三相PR算法===================================*/

/**
 * @brief 三相PR算法初始化函数
 * @param PR 三相PR结构体指针
 * @param Kp Kp 参数，对应速度增益，速度控制速度环
 * @param Kr Kr 参数，对应静态增益，速度控制电流环
 * @param w0 w0 参数，频率
 * @param ts ts 执行周期，单位秒
 * @param wc wc 参数
 */
void ThreePhase_PR_Calculate_Init(ThreePhase_PR_Typedef *PR, float Kp, float Kr, float w0, float ts, float wc)
{
    // 初始化参数
    PR->Kp = Kp;
    PR->ts = ts;

    PR->temp1 = 2.0f * wc * Kr;
    PR->temp2 = 2.0f * wc;
    PR->w0_2_ts = w0 * w0 * ts;

    // 初始化状态变量
    PR->error[0] = 0.0f;
    PR->error[1] = 0.0f;
    PR->error[2] = 0.0f;

    PR->output_of_forward_integrator[0] = 0.0f;
    PR->output_of_forward_integrator[1] = 0.0f;
    PR->output_of_forward_integrator[2] = 0.0f;

    PR->output_of_backward_integrator[0] = 0.0f;
    PR->output_of_backward_integrator[1] = 0.0f;
    PR->output_of_backward_integrator[2] = 0.0f;

    PR->output_of_feedback[0] = 0.0f;
    PR->output_of_feedback[1] = 0.0f;
    PR->output_of_feedback[2] = 0.0f;

    PR->input_of_forward_integrator[0] = 0.0f;
    PR->input_of_forward_integrator[1] = 0.0f;
    PR->input_of_forward_integrator[2] = 0.0f;

    PR->Output[0] = 0.0f;
    PR->Output[1] = 0.0f;
    PR->Output[2] = 0.0f;
}

/**
 * @brief 三相PR算法计算函数
 * @param PR 三相PR结构体指针
 * @param PR_Target 目标电压数组
 * @param PR_Actual 当前电压数组
 */
void ThreePhase_PR_Calculate(ThreePhase_PR_Typedef *PR, float PR_Target[3], float PR_Actual[3])
{
    // A 相
    PR->error[0] = PR_Target[0] - PR_Actual[0];
    PR->input_of_forward_integrator[0] = PR->temp1 * PR->error[0] - PR->output_of_feedback[0];
    PR->output_of_forward_integrator[0] += PR->ts * PR->input_of_forward_integrator[0];
    PR->output_of_backward_integrator[0] += PR->output_of_forward_integrator[0] * PR->w0_2_ts;
    PR->output_of_feedback[0] = PR->output_of_backward_integrator[0] + PR->temp2 * PR->output_of_forward_integrator[0];
    PR->Output[0] = PR->output_of_forward_integrator[0] + PR->Kp * PR->error[0];

    // B 相
    PR->error[1] = PR_Target[1] - PR_Actual[1];
    PR->input_of_forward_integrator[1] = PR->temp1 * PR->error[1] - PR->output_of_feedback[1];
    PR->output_of_forward_integrator[1] += PR->ts * PR->input_of_forward_integrator[1];
    PR->output_of_backward_integrator[1] += PR->output_of_forward_integrator[1] * PR->w0_2_ts;
    PR->output_of_feedback[1] = PR->output_of_backward_integrator[1] + PR->temp2 * PR->output_of_forward_integrator[1];
    PR->Output[1] = PR->output_of_forward_integrator[1] + PR->Kp * PR->error[1];

    // C 相
    PR->error[2] = PR_Target[2] - PR_Actual[2];
    PR->input_of_forward_integrator[2] = PR->temp1 * PR->error[2] - PR->output_of_feedback[2];
    PR->output_of_forward_integrator[2] += PR->ts * PR->input_of_forward_integrator[2];
    PR->output_of_backward_integrator[2] += PR->output_of_forward_integrator[2] * PR->w0_2_ts;
    PR->output_of_feedback[2] = PR->output_of_backward_integrator[2] + PR->temp2 * PR->output_of_forward_integrator[2];
    PR->Output[2] = PR->output_of_forward_integrator[2] + PR->Kp * PR->error[2];
}

/*==================================差分PR算法===================================*/

/**
 * @brief 差分PR算法初始化函数
 * @param PR PR结构体指针
 * @param Kp Kp 参数，对应速度增益，速度控制速度环
 * @param Kr Kr 参数，对应静态增益，速度控制电流环
 * @param w0 w0 参数，中心频率
 * @param ts ts 执行周期，单位秒
 * @param wc wc 参数，带宽频率
 */

static inline void PR_Diff_Reset_Inline(PR_Diff_Typedef *PR_Diff);
void PR_Diff_Init(PR_Diff_Typedef *PR_Diff, float Kp, float Kr, float wo, float wc, float ts, float outmax,
                  float outmin)
{
    if (PR_Diff == NULL)
        return; // 检查指针是否为 NULL
    float fs = 1 / ts;
    float wo_adjusted = 2.0f * fs * tanf(wo / (2.0f * fs)); // 预矫正频率
    float temp1 = 4.0f * fs * fs + wo_adjusted * wo_adjusted + 4.0f * fs * wc;
    float temp2 = (4.0f * Kr * wc * fs) / temp1;
    PR_Diff->Coeff_B0 = temp2;
    PR_Diff->Coeff_B1 = 0.0f;
    PR_Diff->Coeff_B2 = -temp2;
    PR_Diff->Coeff_A1 = (-8.0f * fs * fs + 2.0f * wo_adjusted * wo_adjusted) / temp1;
    PR_Diff->Coeff_A2 = (temp1 - 8.0f * fs * wc) / temp1;

    //  添加比例项kp的影响
    if (Kp != 0.0f)
    {
        PR_Diff->Coeff_B0 += Kp;
        PR_Diff->Coeff_B1 += Kp * PR_Diff->Coeff_A1;
        PR_Diff->Coeff_B2 += Kp * PR_Diff->Coeff_A2;
    }
    PR_Diff->outmax = outmax;      // 输出信号最大值
    PR_Diff->outmin = outmin;      // 输出信号最小值
    PR_Diff_Reset_Inline(PR_Diff); // 初始化状态变量
}
void PR_Diff_Reset(PR_Diff_Typedef *PR_Diff)
{
    if (PR_Diff == NULL)
        return; // 检查指针是否为 NULL
    PR_Diff_Reset_Inline(PR_Diff);
}

static inline void PR_Diff_Reset_Inline(PR_Diff_Typedef *PR_Diff)
{
    PR_Diff->Xn_1 = 0.0f; // 输入信号Xn-1
    PR_Diff->Xn_2 = 0.0f; // 输入信号Xn-2
    PR_Diff->Yn_1 = 0.0f; // 输出信号Yn-1
    PR_Diff->Yn_2 = 0.0f; // 输出信号Yn-2
}